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Abstract 

The paper presents a general method for the resolution of redundancy that combines the 
Jacobian pseudoinverse and augmentation approaches. A direct adaptive control scheme is 
developed to generate joint angle trajectories for achieving desired end-effector motion as well 
as additional user defined tasks. The schenie_cn§ ures arbitrarily small errors between the desired 
and the actual motion of the manipulator. Explicit bounds on the errors are established that 
are directly related to the mismatch between actual and estimated pseudoinverse Jacobian 
matrix, motion velocity and the cont roller, gain. It is shown that the scheme is tolerant of the 
mismatch and consequently only infrequent pseudoinverse computations are needed during a 
typical robot motion. As a result, the scheme is computationally fast, and can be implemented 
for real-time control of redundant robots. A method is incorporated to cope with the robot 
singularities allowing the manipulator to get very close or even pass through a singularity while 
maintaining a good tracking performance and acceptable joint velocities. Computer simulations 
and experimental results are provided in support of the theoretical developments. 


1 Introduction 

The dexterity and versatility offered by redundant manipulators allow their utilization for the 
performance of complex tasks in practical environments. However, effective utilization of this dex- 
terity requires satisfactory resolution of the redundancy and its real-time implementation. 

During recent years two main approaches to the resolution of the redundancy have eftierged. 
These can be categorized as Jacobian pseudoinverse [l]-[9] and Jacobian augmentation [ 1 0]- [ 1 4] 
approaches. In the pseudoinverse approach, a certain vector lying in the null space of the Jacobian 
matrix is utilized for a variety of design objectives. These objectives include optimization of a 
performance criterion [2], obstacle avoidance [3], torque optimization [4], and task prioritization 
[5]-[6]. A review of pseudo in verse methods is given in [7]. In the augmented Jacobian approach, 
an additional Jacobian matrix is defined for the purpose of utilizing the extra degrees of freedom 
offered by redundancy. This matrix is augmented with the end-effector Jacobian matrix to obtain a 
square Jacobian matrix, and thus the problem of redundant manipulator control is transformed to 
that of a non-redundant manipulator. A method to augment the Jacobian matrix for the purpose 
•This work was supported by NETROLOGIC Inc through NASA Grant No. NAS7-1110 
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of optimizing a performance criterion is proposed by Bailoinl [ 1 1]- [1 2] . The concept of augmenting 
the Jacobian matrix is generalized by Seraji [13], allowing the utilization of the redundancy for 
achieving a variety of objectives [1/1]. The augmented Jacobian approach has the feature of making 
the motion cyclic, which is desirable for repetitive operation, and presents an advantage over the 
pseudoinverse approach. However, augmentation introduces additional singularities which cannot 
be easily characterized and which aggravate the singularity problem associated with revolute joint 
manipulators. Desired Cartesian trajectories passing in the neighborhood of such a singularity 
demand very large joint velocities which are impossible to achieve in practice. To overcome the 
singularity problem, methods have been proposed [6], [15] that reduce the joint velocities at the cost 
of introducing or increasing the mismatch between the computed and the actual inverse Jacobian 
matrix. Such a mismatch produces errors in position and orientation when attempting to control 
the manipulator. 

Motion control of a redundant manipulator can be implemented in a hierarchical scheme using 
either of the above two approaches to the redundancy resolution. In such a scheme the joint angle 
trajectories are generated to achieve a desired end-efTcctor motion, as well as achieving additional 
objectives offered by extra degrees of freedom. The generated joint angles are then used as the set 
points of the low level servo-loops. Such a hierarchical scheme is particularly attractive in practice 
since most industrial manipulators have high performance servo-loops that, readily accept joint angle 
set points but cannot easily be modified to implement joint torques in a non-hicrarchical scheme. A 
joint space trajectory generator using the feedback control approach was originally proposed in [16], 
and extended to redundant robots in [17]- [19] within the framework of the pseudoinverse approach 
and in [20] using the augmented Jacobian approach. 

Regardless of the approach used to resolve the redundancy and to overcome the singularity prob- 
lem, the computations involved in motion control of a redundant manipulator can be excessive. 
Motion control using the pseudoinverse approach requires computation of the Jacobian pseudoin- 
verse, its null space matrix, and derivative of an objective function at every control cycle. Similarly, 
motion control using the augmented Jacobian approach, requires determining the Jacobian matrix 
associated with user defined kinematic functions, and the inverse of a higher dimensional augmented 
Jacobian matrix at each control cycle. These intensive computations can make real time implemen- 
tation of motion control on a practical redundant manipulator impossible. 

In this paper we propose a general approach to the redundancy resolution which retains the essen- 
tial features of both the pseudoinverse and the augmentation methods, and which reduces to either 
method as a special case. Within the framework of this general approach, an adaptive kinematic 
control scheme is developed for trajectory t racking that requires only a crude estimate of the inverse 
Jacobian matrix, and thus allows very infrequent computation of the inverse or the pseudoinverse 
matrix. This results in considerable computational savings and makes real-time implementation of 
the scheme feasible on a practical redundant robot. The kinematic control scheme also achieves 
high tracking accuracies and acceptable joint velocities even when the manipulator passes through 
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a singularity. 


2 Adaptive Kinematic Control 

Consider an n jointed robot manipulator performing tasks in the operational space. The 
relationship between the iv e x 1 end-effector position and orientation vector X e , and the n x 1 joint, 
space vector 0, where m e < n, is given by the forward kinematic map 

= /«(©) ( l ) 

The corresponding relationship for velocities is 

* € =Je(0)0 (2) 

where J c (0) = is the m t x n Jacobian matrix of the end-efTector. The problem of kinematic 

control of a redundant robot is to determine the joint angle vector 0(/) to achieve a desired end- 
effector trajectory vector X e d(t), and to utilize the redundancy offered by r ~ n - m e extra degrees 
of freedom to perform additional tasks. In .the pseudoinverse method of redundancy resolution, the 
joint velocity vector Q(t) is related to the en d-eff ector velocity X e by 

0(0 = Ce(0)^(O + 7 (Tn - G e (Q)J c (Q))Z (3) 

where G e (Q) = «/](©) is the pseudoin verse of J e (0) satisfying J e G € J e — Je , JeG e = G t) (G e J e )^ = 
J t G t and {J e G e Y — G t J t) and the argument 0 has been dropped for convenience. The scalar y is 
a positive weighting factor, and Z is an arbitrary n x 1 vector that has no effect on the end-effector 
motion due to the fact that it is multiplied by The null space of J € . In the pseudoinverse method, this 
vector is generally set to the gradient of an objective function ^(0) for the purpose of optimization, 
that is Z = f|p In the generalized augmentation method proposed by Seraji [13], r additional 
kinematic functions ,Y n = /„(©) are defined to resolve the redundancy. These functions are chosen 
to reflect the desired additional tasks to be performed. The rxn Jacobian matrix of additional tasks 
J a (Q) = ^ is augmented with the ?7? e x n end-effector Jacobian matrix J e to form an augmented 
n x n Jacobian matrix. The manipulator now becomes non-redundant. The generalized augmen- 
tation method has the advantage of letting the user easily define additional kinematic functions 
and making the motion cyclic. However, the utilization of redundancy for optimization, although 
theoretically possible within the framework of the augmented Jacobian method, is computationally 
very intensive. This is due to the fact that the Jacobian matrix must be augmented with the matrix 
j. = ^(* t ! I). where N = (/„ — G e J e ) is the null space of the end-effector Jacobian matrix. 

In general, a redundant manipulator can be utilized for achieving two types of tasks. The first 
type, which we will refer to as the primary tasks, are those tasks that can be expressed by % set of 
kinematic equality constraints that must be satisfied accurately. Examples of such tasks are tracking 
an end-effector trajectory, and maintaining an elbow height or a shoulder angle at specified v^ues for 
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the achievement of a certain objective. The other type of tasks, or secondary tasks, involve realizing 
a performance criterion as best as possible, for example, optimizing a performance criterion to avoid 
joint limits. These tasks do not need to be accurately monitored or tightly controlled, and an 
approximate optimization is generally acceptable. In the augmentation method all additional tasks 
are treated as primary tasks that must to be achieved accurately. An excessive number of primary 
tasks will result in the difficulty or inability in achieving these tasks. On the other hand, in the 
pseudoinversc method, the tasks are formulated in the form of an optimization criterion, whfeh may 
not be the most practical or natural way of expressing and solving a particular kinematic problem. 

To ease these trade-offs, we propose a method for combining augmentation and pscudoftv verse 
approaches, thereby permitting a more natural formulation of both kinds of tasks. Specifically, let 
0 < r a < r degrees of redundancy be used for the user defined tasks described by the r a x 1 vector 
equation X a — /<,(©), where X a is the additional task vector. The augmented system of kinematic 
equations is 

X = 

where X is an m x 1 vector, m = ( m e 4* r fl ), and will be referred to as the "posture” vector. This 
vector is composed of position and orientation of the end-efiector, and possible additionafVinematic 
functions that define Cartesian or angular position of the arm such as elbow height or distance of 
points on the arm from obstacles. The equation relating A” to 0 is 

X = JO (5) 


_ ( /e(©) 

” V /..(©) 


where J = y ^ j is the m x n augmented Jacobian matrix. Similarly, let 0 < v Q < r degrees 
of freedom be utilized for optimization of a performance criterion ^(0), where r — r a + rV The 
optimization criterion can be manipulability maximization, joint limit avoidance, minimal joint 
motion or obstacle avoidance, to mention a few. Equation (3) can now be used to obtain the joint 
angle trajectory vector 0(0 for achieving both a desired posture vector Y<f(0 and optimization of 
the performance criteria ^(0). This gives 


0(0 = f (GX d {t) + 7 ■(/« - GJ)Z ) dt 


where G = Jf(0) and Z = . It is evident that when r 0 — 0, the method reduces to the purely 

augmented Jacobian matrix method. Likewise, when r a - 0, it reduces to purely pseucloin verse 
method. 

Equation (6) is of little practical use since the integration can drift away even with small inac- 
curacies in the knowledge of the kinematic parameters or the computation of the Jacobian matrix. 
It will also produce undesirable behavior when the manipulator passes close to a singularity. It 
is, therefore, important to develop a scheme that is robust with respect to robot singularities or 
inaccuracies in the estimation or computation of the Jacobian matrix. The latter is very important 
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Figure 1: Block diagram of the overall scheme. 

since the computation of the Jacobian inverse or the pseudoinverse must be performed very infre- 
quently to reduce computational burden. Such infrequent computations generate inaccuracies in 
the inverse Jacobian matrix and are reflected as errors in the manipulator position and orientation. 
In order to resolve these difficulties, the feedback control scheme of Figure 1 is employed, where 
E(t) = - X(t) denotes the error between the desired and actual posture vector. The controller 

consists of an adaptive time-varying feedback gain matrix K(t) acting on the error and a matrix G 
that represents an estimate of the pseudoinverse Jacobian matrix G = J*. The estimate G can be 
considerably different from the actual G, although a good estimate reduces the controller gain, as 
will be seen. As we discussed earlier, our objective is to achieve accurate tracking of the end-efFector 
and the additional tasks as defined by the desired value of the posture vector while attempt- 

ing to satisfy the secondary tasks as best as possible. Consequently, the error in the posture vector 
is directly controlled, whereas the optimization is indirectly realized. Figure I indicates that the 
required joint angular velocity vector 0(t) can be obtained from 

0(0 = g (aoco + mm) + 7 (/« - gj)z ( 7 ) 

where J is an estimate of the Jacobian mat rix. The problem is now to determine K(t) to ensure that 
the posture vector X(t) closely follows its desired value Xd(t) so that the error /?(/) is arbitrarily 
small. Premultiplying (7) by J and substituting the result into (5), we obtain 

X(t) = JG [.V d (0 + A'(0£(<)] + 7 J(In - GJ)Z (8) 

Subtracting both sides of (8) from X d and rearranging yields the error dynamic equation 

E{i) = - JGK{t)E{t ) + (/,„ - JG)Xd(t) - 7 J(I« ~ GJ)Z (9) 

Let us define the mismatch between the actual and estimated Jacobian matrices as 

H = 7 m - JG (10) 
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In the ideal case where J is known accurately and rank J — m y the mismatch matrix is II — 
I m - JJ T (JJ T )~ l = 0, and there is no mismatch. Substituting (10) into (9) and simplifying, we 
obtain 

E(t) = -K(t)E(t) + HK(t)E(i) + If X (i (t) - jHyz (11) 

where Hi = (J — J) + IIJ is a modified mismatch matrix and has the property that H\ = 0 whenever 
the original mismatch matrix II is zero. The term jH\z is in fact the interaction of the secondary 
task on the primary task error dynamics. When II = 0, (11) simplifies to E(t) = — K(t)E(t). In 
this case the nonlinear system (11) is reduced to a simple linear system, which partially explains 
the reason for using the feedback configuration of Figure 1. It is easy to show that in the case of 
zero mismatch, lim t -. 0O E = 0, provided that K(t) is a symmetric positive definite matrix. However, 
there will always be a mismatch because of the imperfect knowledge of the robot parameters, the 
inaccuracies due to infrequent calculations of the inverse Jacobian matrix, or the robot operating 
near a singular point. It is, therefore, desirable to develop a posture trajectory tracking algorithm 
that will be robust to both inaccuracies in the Jacobian matrix calculations and robot singularities. 
In the next section, we will show that the control algorithm (7) with K(t) designed as an adaptive 
proportional plus integral controller will achieve these objectives. 

3 Stability and Tracking Performance 

Consider the proportional plus integral feedback matrix 

K(t) = Kp+Ki(t) (12) 

where Kp is a constant positive definite symmetric matrix, and Kj(t) is obtained from an integral 
adaptation algorithm as 

K,{t) = A'o + f (aE(r)E T (T) - <tK,(t)) dr (13) 

Jo 

where A'o is a constant positive definite symmetric matrix representing the initial value of the 
integral, a > 0 is the constant integral coefficient, and a > 0 is the leakage coefficient used to avoid 
possible integral wind up [21]. Equation (13) implies that 

A'/(<) = aE(l)Ef r (t)-<rK,(t) (14) 

Note that Kf(t) is a positive definite symmetric matrix for all t. In order to show that the error 
E(t) described by the dynamical equations (11) and (14) can be made arbitrarily small, we consider 
the Lyapunov function candidate 

V = ±E T (t)E(t) + Xtr(Ki(l)K,(t)) ' (15) 

whose derivative along the trajectories of (11) and (14) is 

= -E t KE + E t HKE + E T II X 4 - 1 E T H l Z + ~ tr (l<Ji<,) (16) 
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where the time argument is dropped for convenience. Substituting for K and K from (12) asd (M) 
into (16) and simplifying, we obtain 

V = -E t ((1 - ±)K, + A>) E + E T HKE + E T nX A - 1 E T n x Z - ^ tr(KfK,) (17) 

Suppose that the coefficient a is chosen such th at a > 1, and let fci — ^min (AVH-(l-i)A'/). 
and At 2 = A m<1 *( A’), where A mtn and \ niaI denote the minimum and the maximum eigenvalues of a 
matrix, respectively. Equation (17) implies that 

V < -Jfc,||A|| 2 + Ar 2 ||//||||A’|| 2 + E T ( II X d - yll x z) - ~ tr (KjK,) (18) 

Let i] - maa.’(||//||), J] X = mar(||//i||), < = maz(||A' d ||), and p = max{\\Z\\). We have in the worst 
case 

V < -1*1 ^1 - jh»?) ||A|| 2 + (r?c + Tw)||E|| - ^ tr (I<J K/) (19) 

In order to prove the stability of the error dynamics, we will assume that i] < < 1. This 

assumption places an upper bound on the mismatch. It must be noted, however, that this upper 
bound is resulted from the worst case nature of the analysis, and that in practice the scheme often 
accommodates larger mismatches, as will be demonstrated in Section 4. Let 

k = A *1 ^1 - jhjj) ; <7 = jjC + yr} X p (20) 

to obtain from (19) 

V < -k II All 2 + q II All - ^ tr [ Kj K,) (21) 

Substituting for ||A|| 2 = 2V - 4, tr ( Kj K,) from (15) into (21) and dropping the negative term 
appearing under the radical in the resulting equation, we obtain 

v < -2kv + q Vw - tr ( K J K >) ( 22 ) 

Now if the leakage coefficient is selected such that a > k, then (22) reduces to 

V<-2kV+q\/2V (23) 


Note that the choice of a > A* is very conservative due to the fact that the negative term was dropped 
above and that in practice a much smaller value of <x is sufficient to ensure the validity of inequality 
(23). This inequality is now in a form that can be solved analytically by defining 


W = W - 


1 

y/2le 


(24) 


which yields 



(25) 
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Substituting (24) and (25) into (23) and dividing both sides of the resulting inequality by 2>/V > 0, 
we obtain 

\V < ~kW (26) 

The solution to (26) is 

W{E) < IV(£(/o))c-*f , "'°) (27) 

where to is the initial time. In view of (24), the last inequality becomes 



It. is easy to verify that V(E) decreases monotonically along any solution of the error dynamic 
equations (11) and (14) until the solution reaches the compact region 

Kj = {e: V{E)< Vj = ^ (29) 

where the subscript / denotes the final values, that is as t — + oo, The rate of decrease of V(E) is 
at least as fast as e~ ki . We conclude that the solutions E of (11) and (14) are globally ultimately 
bounded . Since from (15) we have ||Aj| 2 < 2V, it follows that within the region 7 Zj given by (29), 
the error is bounded by 

l|£||<f (30) 

Equations (30) and (20) indicate that the upper bound on the steady-state tacking error is related 
to the accuracy of the inverse Jacobian matrix estimation, the desired velocity of the posture vector, 
and the controller gain. The adaptive nature of A' coupled with a reasonable estimate of the inverse 
Jacobian allows the achievement of very low tracking error with small or moderate controller gains. 
Extensive simulation results have shown that even when the estimate of the Jacobian matrix is 
significantly different from its actual value and the end-effector moves fast, the error can be made 
very small by increasing the controller gain k. The latter can be achieved using higher values of the 
integral coefficient cv and the constant matrices A*o and Kp. 

3.1 Discrete-Time Implementation 

In practice, the adaptive trajectory generator algorithm described by (7), (12) and (13) is 
implemented in discrete time. Using the trapezoidal integration rule, the discrete form of (13) is 

Kn = K 0 + ^J2 [tv (EjEj + E j+i Ej +l ) - a (A'/j + A', (/+1) )] (31) 

_ 1 j - o 

where T c is the control sample time and Kn and Ei denote , respectively, the values of the adaptive 
matrix and the error vector E(t) at time t = fT C) ? = 0,1,2,... . As with any discrete time 

feedback systems, the tracking performance deteriorates when T c is increased. Equation (31) can be 
written in a computationally more convenient form as 

Kn = ( - ^ 2—— ) [(i _ 0.5<rT e )A'/(i_i) + 0 .5aT c (EiEff + E^EU)} (32) 
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( 33 ) 


Similarly, the discrete form of the control law (7) using the trapezoidal integration rule is 


where 


0, - 0, - j + “(ft* + fii - 1) 
i = Gi (a'.y, + Kit-:,) +7 (In - GiJi)Zi 


(3d) 


where the subscript i denotes the values at time / = ?T C . The above control law requires an estimate 
of the Jacobian pseudoinverse matrix G,*. An efficient algorithm to calculate the Jacobian matrix ./,* 
is given in [22]. The estimated pseudoinverse matrix is obtained from Ji as 


Gt = ji 1 " (u T y l 


(35) 


where the pseudoinverc in (35) minimizes ||A\- - .7,0,11- This procedure will provide a reasonably 
accurate estimate of the pseudoinverse, that is C « (7. However, it has two main drawbacks, namely 
computational intensity and excessively large values of (7, and consequently of joint velocities, near 
the robot singularities. In order to resolve the problem of excessive joint velocities near the robot 
singularities, we estimate the Jacobian pseudoinverse to minimize 


{ ||A'i - Ji 0.|| + P II0.II } 


(36) 


where the value of j3 > 0 determines the weighting placed on the minimization of joinjt velocity 
errors. The inverse Jacobian matrix that realizes (36) is [6] 


Gi = Ji T (Ji Ji T + pi) 


-1 


(37) 


Note that the inverse in (37) exists even when the manipulator is at a singularity. The weighting 
factor 0 can be adjusted to have a small value near robot singularity, and zero elsewhere. One 
method to achieve this is to choose 0 according to 


P = 


0o( 1 “) 2 1 W < w o 

Wo 

0 , w > Wq 


(38) 


where w = y/det(J J^) is the manipulability measure, 0 O is a constant and u>q is a specified threshold. 
A more elaborate technique for the selection of 0 is based on singular values of Ji [3]. The addition 
of the term 01 in (37) introduces mismatch in the Jacobian pseudoinverse and can produce tracking 
errors. However, the proposed adaptive kinematic scheme is robust with respect to the mismatch and 
will automatically increase the gains to the level that is needed to achieve good tracking performance, 
as discussed before. 

A major portion of the computation in each control cycle is the calculation of the Jacobian matrix 
and its pseudoinverse. In order to significantly reduce the computational burden, the calculation of 
the Jacobian matrix and its pseudoinverse are performed, merely once every Tj = vT c seconds, where 
v 1 (typically 100) is an integer. This implies that the costly computation of the Jacobian matrix 
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and its pseudoinverse are performed only a few times during a typical robot motion. Furthermore, 
to account for the delay time that occurs due to the computation of (7,-, we introduce a vT c second 
delay in the implementation of the scheme. Equation (3d) is now 

fi, = Gt - 1 [X M + Ki /?.) + 7( T n ~ Gi - 1 h - 1 Vh (39) 

where Gi~\ is the value of G at time i = (/ — 1)7} = (/ — 1 )/'T c , / — 1,2,3, * - . Because of 
the robustness of the adaptive kinematic algorithm to the Jacobian mismatch, accurate tracking is 
possible despite the mismatch which is brought about, by both the infrequent updating of Jacobian 
pseudoinverse and the computation delays. This will be demonstrated in the next section. 


4 Examples 

In this section, the adaptive joint space trajectory generator developed in the previous sections 
will be applied to two examples. In the first example a modified PUMA 562 is utilized to demonstrate 
the redundancy resolution using the proposed adaptive kinematic control method. In the second 
example, the kinematic model of a regular PUMA 562 manipulator is used for both position and 
orientation tracking while the robot passes repeatedly through singularities. The control sample 
time is selected as T c = 2 ms in all cases. 


4.1 Example 1 

In this example we investigate the performance of the adaptive scheme for trajectory tracking 
of a modified PUMA 562 manipulator. The PUMA is made redundant by adding an extra link to 
its wrist, as shown in Figure 2. The joint angles to be used are waist 0 shoulder $ 2 > elbow 0 3 , 
and wrist angles Q.\ and 0$. Joint 6 provides rotation of the extra link and will not be used in this 
example. The modified PUMA is now a redundant for positioning the tip of the extra link in the 
three dimensional space. The kinematic equations of the modified PUMA, which has two degrees 
of redundancy, are given in the Appendix. In the following we will study two cases, where in both 
cases the tip is required to move on a straight line in the Cartesian space. The desired position 
trajectory is described by 


x ed (i) = X c (0) + (A'c(oo) - X e (0)) g(t) 


(40) 


where X e (0) and ,Y e (oo) are the initial and final values of the tip position vector, and g(.) is a 
cycloidal function of the form 


9{v) = 


V 

T 


l, 


1 . 27ri> 

— sin , 0 < v < r 

2tt t 

V > T 


(41) 


where r is the time required to move the tip from its initial position AT e (0) its final position AT c (oo), 
and is selected as r — 2s. Note that (40)-(41) describe the equation of a straight line in the Cartesian 
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coordinates. The starting joint angle vector is selected as 0(0) = (-'15, —20, —5, 0, 50) T degrees. 
These correspond to the tip Cartesian position A' e (0) = (.140, -130, 810) T mm The final position 
of the tip is specified as .Y c (oo) = (500, 500, 50O) T mm , and the Cartesian distance to be traveled 
is D = ||A'e(oo) - A r c (0)|| = 720 mm. 


4.1.1 Case A 


In this case study, combined augmentation and optimization are utilized for the resolution of 
the redundancy. In addition to the tip trajectory tracking, it is desired to decrease the elbow height 
linearly from the starting value /i(0) = A’jj(O) = 150 mm to reach a value of h(o o) = X a (oo) = 0 
at time / = Is. This height is to be maintained to avoid collision with an obstacle while the tip 
continues its motion to get to the goal position at. time t = r = 2 s. The desired elbow height 
trajectory is 

X ad (t) = A'„(0) + (A'a(oo) - (A' fl (0)) g(2t) (42) 


where g(-) is given in (41). The desired posture vector is obtained by augmenting the tip position 
vector and the elbow height as 

X d (t) = f Y*d(<) ) = x 2,r(0. x 3 d(t), x 4d(t)) T (43) 

where the first three components describe the Cartesian coordinates of the tip position, and the last 
component, x. 4 d(/), is the desired elbow height trajectory. 

In addition to the above specifications and in order to avoid joint limits, it is desired to keep 
each joint angle 0j , j = 1,2, •••,5, as close as possible to its center value 9j c = , where Qj u 

and 0j t are the upper and lower limits of the joint angle 0j . This is done by maximizing the function 


*(©) = - 



(«) 


where Qj s = 2 span. The set of pairs for joints 1 to 5 of the PUMA are, 

respectively, (-160, 160), (-223,43), (-48,236), (-110, 170) and (-100, 100) degrees. 

The control algorithm (32), (33) and (37)-(39) are applied in which the gain matrix h(t) is 
adapted with K P = A'o = 0, a = 10 9 and <r = 0.7 , so that only integral adaptation with a 
leakage term is employed. Note that the large value of a is due to the fact that the errors Ei 
and the sample time T c in (32) are measured in meters and seconds, respectively. 1 . The values 
of /? 0 = 0.007 and u? 0 = 0.015 are used in (38). The pseudoinverse Jacobian matrix is computed ) 
every Tj = 100 ms. This means that for every 50 control cycles only one pseudoinverse Jacobian 
is computed. In addition, to account for the delay in computation, a one sample delay bf Tj is 
introduced in the calculations of the Jacobian pseudoinverse matrix, as discussed bcf° re ' Thus the 

1 Although the results are given in millimeter, milliseconds and degrees, the actual computations are performed 
with units in meters, seconds and radians 
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pseudoinverse being used for computing of (lie joint angles is out of date by as much as 200 ws, 
which can produce large mismatch. 

The arm motion is shown in Figure 3 . The actual tip position trajectories (x’l (/), x^t), x 3 (t)) 
and the elbow height trajectory x.i(/) closely follow the desired trajectories (x*i<i( 0 > x ?d(t)> - r 3 rf( 0 ) 
and respectively, as seen in Figure A. The errors C[(t) = xu(t) — x\ (t), eo(t) — X2d(0 — X2(0» 

e 3 (t) = x 3 d(t) — a: 3 (2) and e^(t) = ) — x.^(t) are plotted in Figure 5 and indicate maximum 

errors of 1.0 mm, 0.7 777777 , 0.6 mm along the three axis, and 1.05 mm in the elbow height. These 
maximum errors are very small compared to the traveled distances shown in Figure A. The average 
errors are less than 0.3 mm. It must be noted that the above low tracking errors have been obtained 
despite relatively fast trajectories and infrequent .Jacobian matrix computation. The joint angle 
trajectories to achieve the above motion is shown in Figure 6. The joint angles have been kept as 
close as possible to their center values through the optimization of the performance criterion. It 
is interesting to note from Figures 3 and 6 that the arm continues to change posture to further 
optimize the performance criterion even at the completion of the tip trajectory. This is done while 
keeping both the tip at the position A e (oo) = (500, 500, 500) T mm and the elbow at the height of 
A" a (oo) = 0. The optimization can be speeded up and distributed more evenly throughout the arm 
motion by increasing the value of 7 in (15). 

Suppose now that to further reduce the computations, the Jacobian pseudoinverse matrix is 
initialized correctly, but is not updated at all, that is Tj — 00 . The norm of the mismatch matrix, 
||//||, is plotted in Figure 7 and indicates a maximum mismat ch of ?; — mf/x||/7|| 2.8 which exceeds 

the conservative limit of 1 used in the proof of stability. Despite this, the error responses plotted 
in Figure 8 show a maximum error of only 1.3 777777 , 0.8 777 m and 1.3 777777 . in the three coordinates 
of the tip position and 0.3 mm in the elbow height. These error are only slightly higher than those 
obtained with 100 ms updating. It is seen that despite suspension of the Jacobian corrfXitations, 
which has resulted constant Jacobian matrix through the entire motion, the scheme has tracked the 
desired trajectories accurately. This has been achieved mainly due to the adaptive control scheme. 

Finally, let us quantify the computational savings as a result of infrequent updating. Let the CPU 
time required to compute the joint trajectory vector 0 for one control cycle with Jacobian updating 
times Tj = T c = 2 7??s, Tj = 100 ms and Tj = 00 be denoted by 2 mo and ^oo> respectively. Using a 
Sun computer it is found that for the present example, I 2 = 6.5 771s, *mo = 106 777 s and — 1.00 777s. 
These results reflect the computational intensity involved in the Jacobian pseudoinverse calculations 
despite using an efficient algorithm [23]. The results also indicate considerable CPU time savings due 
to infrequent updating of Tj — 100 ms. Although the adaptive scheme can tolerate no pseudoinverse 
updating, only little extra savings is obtained using Tj — 00 . 
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4.1.2 Case B 


The purpose of this case study is to show how both extra degrees of freedom can be used for 
augmentation, and to further demonstrate the robustness of the algorithm. 

Consider the manipulator where the tip and the elbow are required to track the trajectories as in 
Case A. In addition, these tasks are to be performed while freezing one wrist joint angle to:a value 
of 0.\ — 45° to represent the case of a joint motor failure. 'The additional task vector is now the 2 x 1 
vector X a — ^ ^ where x 5 = 0 4j and the posture vector is X = (X e X a ) T = (xi, x 2 , • • • ,x 5 ) T . 

All parameters are left at their values as in Case A. The error responses are shown in Figures 9 
and 10 and indicate low tracking errors in all components of the posture vector. The maximum 
Cartesian error occurs in x y with a value of t\ = 0.9 mm, and the average error is less than 0.4 mm. 
The maximum wrist angle deviation from its frozen value is 0.0 degree. The joint angles trajectories 
to achieve the above tasks are shown in Figure 1 1, and reach their final values after about 2 s when 
the desired trajectories are their steady-state values. Comparing Figures 11 and 6, we observe that 
the joint angles in Figure 11 are farther away from their center values because no optimization is 
performed in Case 13. The arm motion is depicted in Figure 12 and further illustrates this point. 

If the inverse Jacobian matrix is not updated, the maximum Cartesian error is found to be 
1.05 mm, and the maximum wrist angle error is 0.3 degree, which are about the same values with 
Tj = 100 ms. The CPU times for one control cycle computation are t 2 = 6.0 ms, hoo = 0.80 ms 
and — 0.62 ms for Jacobian update times of 2 777 s, 100?7?s and 00 , respectively. These values are 
close to those obtained in Case A, and further illustrate the significant CPU time savings as a result 
of infrequent updating. 

4.2 Example 2 

The purpose of this example is to demonstrate the capability of the adaptive scheme to perform 
both position and orientation trajectory (racking in the standard PUMA 650 robot in the difficult 
situation where the manipulator is required to perform a complex motion that passes through a 
robot singularity repeatedly. Orientation trajectory tracking is generally hard to achieve due to 
the presence of many singularities associated with the wrist. The end-efFector vector is denoted 
by X = (xi, x 2 , — ,x 6 ) T , where x ls x 2 and x 3 are the position components, and x 4> x 5 ^nd x 6 
are the orientation components. Here, all degrees of freedom are used for the primary taskfe. The 
end-effector coordinate vector X is related to the angles of waist 0 U shoulder 0 2) elbow # 3 , and the 
three wrist angles 0 \ , $5 and 0 $ through the forward kinematic equations given in the Appendix. 
The orientation is described by the equivalent axis representation [24]. 

The desired end-effector trajectory is selected as shown in Figure 13. The PUMA manipulator 
starts at 0\ = 0 2 - 0 3 = 0 4 = 0 6 - 0, and 0 5 = 30° with the corresponding end-effector position 
(xj , x 2 , x 3 ) t = (440, 149, 481) r mm, and the orientation (x 4 ,£5 , X 6 ) r = (0, 30, 0) T degrees. 
The end-effector takes a partial spiral path until it reaches a tilted circular path of a specified radius. 
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It then continues its motion on this circular path repeatedly. The desired end-effector orientation is 
such that it holds an object upward (e.g. holds a glass of water without spilling the water) wljile the 
tilted path is traversed. In order to specify this tilted spiral-circular motion, we defined the rotation 
matrices: 
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and orientation vectors p and and q as 


/ rsiii(^d) \ / i-.i(oo) - a-.i(O) \ 

p(t) = R 2 Ri r cos(2ii(/) ; q = * 5 ( 00 ) - »s(0) (46) 

\ 0 / V ^e(oo) - x c (0) J 


where d = ±1 specifies the direction of rotation. Note that p(t) describes the equation of a circle 
that is rotated about axis X\ and xo. The desired trajectory vector is now 

X d (t) = -Y(0) + ( J ) <700 (47) 

where g(.) is given in (41), and has the effect of smoothly taking the end-effector from the starting 
position and orientation A r (0) into a spiral path and finally to a circular path with the desired tilt. 
The parameters in (45)-(46) arc selected as \ p\ = 35 degrees <po = 20 degrees, r — 300 mm, r = 5 s 
and d = — 1 . 

The parameters are I\o — Kp = 0, a = 10 7 , cr = 1.5, /?o = 0.01, u>o = 0.025 and the Jacobian 
matrix update time is Tj ~ 100 ms. The determinant of the Jacobian matrix is shown in Figure 
14, and clearly changes sign, showing that the manipulator passes through a singularity repeatedly. 
The position and orientation errors are given in Figures (15) and (16) . The maximum position and 
orientation errors are about 3.5 mm and 0.7 degree, and occur along x\ and x.\, respectively, and 
occur at the singularity point. The joint velocities are plotted in Figure 17 and show a maximum 
value of less than 120 degrees/s as the arm goes through singularities. It is seen that despite 
infrequent Jacobian updating and the complex manipulator motion passing through singularities, 
the adaptive kinematic control scheme has produced low errors and velocities. The CPU time savings 
due to infrequent updating is again significant in this case with ti — 7 ms and t ioo — 1.0 ms. 

It must be emphasized that the above two examples demonstrate typical performance of the 
proposed adaptive scheme. Similar results were obtained for a wide range of desired trajectories 
with different velocities and with very infrequent Jacobian pseudoinverse computation^ l. Higher 
tracking accuracies are achieved by more elaborate tuning of the controller parameters Kp u A'o, oc 

and a. _ 

Finally, the scheme was implemented on both the standard and modified PUMA 562. The 
control software was written in the C language on a 486 PC. A serial link was established for the 
communication between the higher level adaptive controller residing on the PC and the PUMA joint 
level controller. Several examples similar to the above were successfully implemented. 
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5 Conclusions 


A method is proposed for redundancy resolution that generalizes Jacobian pseudoinverse and 
augmentation approaches. The method is flexible and allows the user to easily define tasks involving 
trajectory tracking and performance optimization. An adaptive algorithm for solving the inverse 
kinematic problem of redundant robot has been presented which uses a feedback loop with an adap- 
tive controller to generate the joint angle trajectories for achieving desired end-effector trajectories, 
as well as other desired posture trajectories defined by the user. 

It is shown that the errors in posture trajectories are globally ultimately bounded for different 
velocities and bounded uncertainties in the estimation of the Jacobian pseudoinverse matrix. This 
robustness allows very infrequent pseudoinverse computations and make the scheme computationally 
fast and suitable for real-time implementation. A further feature of the scheme is its tolerance to 
manipulator singularities and allows the robot to go close to or even pass through singularities while 
maintaining acceptable joint velocities and low tracking errors. 

The scheme has been applied to a practical redundant robot for achieving both trajectory tracking 
and optimization and the results show the effectiveness of the scheme under a variety of conditions. 
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7 Appendix - Forward Kinematic Equations 

In this appendix we present the forward .kinematic equations of the five DOF redundant ma- 
nipulator used in Example 1, and the 6 DOF PUMA 562 used in Example 2. 

Consider the forward kinematics of the modified PUMA 650 manipulator shown in Figure (ref- 
fmarmpic.ps). Let Q\ , 0 2 and be the waist, shoulder and elbow joint angles, respectively, and use 
the standard notations c\ = cosd i, s, = sin0\, c \ 2 = cos(0\ -\-0 2 ), etc. The shoulder position vector 
P>h is : 

/ -rf.«i \ 

Pth = I diC| j ' (d8) 

where d\ = 149.1 mm is the base to shoulder length . The unit vector a 2 = defines the 

reference axis for i 9 2 - The elbow position vector is 

( h c \C 2 ~ /3C23C1 \ 

I 2 S 1 C 2 J (49) 

“ h s 2 + / 

where / 2 — 432 777771 is shoulder joint to elbow joint distance and / 3 = 20.3 777777 is the elbow offset. 
The elbow height is h — -l 2 s 2 . The wrist position is 

( d\C\S 2 ^ \ 

d\ s \ 5 2 3 I • (50) 

<Uc 2 3 / 

where d 4 = 432 mm is elbow to wrist length. The unit vector ( 14 — defines the reference axis for 
0 4 . To find a 5 , the unit vector for 0 5 , we note that due to the PUMA 562 geometry, a 5 is parallel 
to a 2 when the manipulator is at ’’home” position and is zero. Consequently, the unit vector a 5 
is obtained as a 5 = rot( 0 4 ,a 4 )a 2 where rof(^,a 4 ) is the matrix representing the rotation about the 
vector (14 by O 4 , and is obtained from 

ro/(0 4) a 4 ) = C 4 I + ( 1 — c.i)n 4 aj + 54 ^( 04 ) (51) 

where S(.) is the skew symmetric operator defined so that for two vectors a and 6 , a x b = S(a)b . 
Since a 6f the reference axis for joint is parallel with a 4 when #5 = 0, we have a$ = roi(d^ i a^)a 4 
which rotates a A about a 5 by 0 5 . The position of the end-efTector mounting plate is 

Pc = h a 6 + Pxur (52) 

where / 5 = 57.2 mm is the wrist to plate length. When the extra link is mounted on the end-effector 
plate, the tip position of this link is 

Xtip — (h + ^6 )^6 + Pwr ' (53) 

where Iq = 250 mm is the length of the extra link. 
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The orientation of the plate is determined using the equivalent axis representation [24]. This 
requires computing the rotation matrix 

R = rot(0 6l a 6 ) rot(0 5 ,a 5 ) rot(0 4} a A ) rot(0 3 + rot(0 \ , a { ) (54) 


Note that c 1 2 = a 3 since the axis of rotation of joints 2 and 3 are parallel. Given the rotation matrix 
/?, the angle of rotation $ and the axis of rotation k are obtained from [24] 


-d = cos 1 


^( r ll + r 22 + ^33 - 1) 


(55) 


where rn> • * • , ?’33 are the elements of the rotation matrix R. The end-effector orientation is* 


f*3 2 “ ^23 
^ 13-^31 

r 2i - ri2 


d < 7r — e 


(56) 


where e is a small number and 


w = < 


2 sin i? 
1 


l 2’ 


, € < 0 < 7T - € 


d < € 


(57) 


In order to determine <j> e for $ > tt — e, let = \J rn ^ u i = \ an d u i — Then it 


«i 


£ 21 . 

2tio 


can be shown that [26] <f) e = i) I for $ > tt — c and |uj | > e; <£ c = t?l t /2 for t? > 7r — e, 


£ii 
2u i 


£2i 

2u 3 


£U- 


| u 1 1 < e and 1 1/2 1 > and <f> e = ’d ^ j for all other values of d. Finally, the end-effector 


^3 


position and orientation vector is 


•-(£> 
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